#include<rclcpp/rclcpp.hpp>
#include<rclcpp_action/rclcpp_action.hpp>
#include<iostream>

#include"turtle_square/action/rotate_and_move.hpp"
using RotateAndMove=turtle_square::action::RotateAndMove;
using GoalHandleRotateAndMove = rclcpp_action::ClientGoalHandle<RotateAndMove>;

class TurtleSquareClient : public rclcpp::Node
{
public:
    TurtleSquareClient():Node("turtle_square_client"){
        //创建action客户端
        client_ =rclcpp_action::create_client<RotateAndMove>(this,"turtle_rotate_and_move");

        //启动画正方形
        start_square_path();

    }

private:
rclcpp_action::Client<RotateAndMove>::SharedPtr client_;
int current_side_=0;
const int total_sides_=4;
const float side_length_=2.0f;
const float turn_angle_=1.57f;

void start_square_path()
{
    if(!client_->wait_for_action_server(std::chrono::seconds(5))){
        RCLCPP_ERROR(this->get_logger(),"Action Service Is Not Available");//如果服务端没有响应，就打印这条信息
        rclcpp::shutdown();
        return;
    }

    RCLCPP_INFO(this->get_logger(),"我开始画正方形啦！边长：%.2f,旋转90度",side_length_);

    send_next_move();

}

void send_next_move(){
    auto goal_msg=RotateAndMove::Goal();

    if(current_side_==0){
        //第一条边：只前进，不旋转
        RCLCPP_INFO(this->get_logger(),"第1边：前进%.1f单位长度",side_length_);
        goal_msg.target_angle=0.0f;
        goal_msg.target_distance=side_length_;
        goal_msg.rotate_first=false;//先移动

    }
    else{
        RCLCPP_INFO(this->get_logger(),"第%d边，旋转90度，前进%1f单位长度",current_side_+1,side_length_);
        goal_msg.target_angle=turn_angle_;
        goal_msg.target_distance=side_length_;
        goal_msg.rotate_first=true;//先旋转
    }
    //设置移动参数（角速度，线速度，最大容差值）
    goal_msg.linear_speed=1.5f;
    goal_msg.angular_speed=1.0f;//旋转素的
    goal_msg.distance_tolerance=0.05f;
    goal_msg.angle_tolerance=0.05f;

    //向服务端发送目标
    auto send_goal_options=rclcpp_action::Client<RotateAndMove>::SendGoalOptions();
    send_goal_options.goal_response_callback=std::bind(&TurtleSquareClient::goal_response_callback,this,std::placeholders::_1);
    send_goal_options.result_callback=std::bind(&TurtleSquareClient::result_callback,this,std::placeholders::_1);
    
    client_->async_send_goal(goal_msg,send_goal_options);
}
void goal_response_callback(GoalHandleRotateAndMove::SharedPtr goal_handle)
{
    //目标处理：查看服务器是否接收到指令
    if(!goal_handle){
        RCLCPP_INFO(this->get_logger(),"目标被服务器拒绝");
        rclcpp::shutdown();
    }else{
        RCLCPP_INFO(this->get_logger(),"目标已经被接收了！执行中...");
    }

}
//检查执行情况
void result_callback(const GoalHandleRotateAndMove::WrappedResult&result)
{
    switch(result.code){
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(),"第%d边完成：%s",
                current_side_+1,result.result->message.c_str());
            break;
        
        case rclcpp_action::ResultCode::ABORTED://目标被中断
            RCLCPP_INFO(this->get_logger(),"目标被终止：%s",
                result.result->message.c_str());
            break;
        
        case rclcpp_action::ResultCode::CANCELED://目标被中断
            RCLCPP_INFO(this->get_logger(),"目标被取消：%s",
                result.result->message.c_str());
            rclcpp::shutdown();
            return;

        default:
        RCLCPP_INFO(this->get_logger(),"发生了未知错误！");
        rclcpp::shutdown();
        return;
    }
    //移动到下一边
    current_side_++;
    if(current_side_<total_sides_){
        send_next_move();
    }else{
        //画完了
        RCLCPP_INFO(this->get_logger(),"正方形已经画完了！");
        rclcpp::shutdown();

    }

}
};

int main(int argc,char**argv){
    rclcpp::init(argc,argv);
    //创建客户端节点
    auto client=std::make_shared<TurtleSquareClient>();
    //运行节点直到完成所有动作
    rclcpp::spin(client);

    rclcpp::shutdown();
    return 0;
}
